function [PathPoints, st, ErrCode, info] = LPLNSNPath(vehx, vehy, vehth, tpmx, tpmy, tpmth,R1, R2, PSInfo, VEHInfo,ParamInfo)
% 左转前进（大圆） + 左转后退（小圆） + 直行后退

% 输入数据处理
Cr2SafeDist = ParamInfo.Cr2SafeDist;
ymin = ParamInfo.ymin;
NR = ParamInfo.NR;
cr2x = PSInfo.cr2x;
cr2y = PSInfo.cr2y;
VehWidth = VEHInfo.VehWidth;
VehRear  = VEHInfo.VehRear;
VehLength = VEHInfo.VehLength;

VehWidth = VEHInfo.VehWidth;
Cr2SafeDist = ParamInfo.Cr2SafeDist;
Cr2obsx = PSInfo.cr2obsx;
Cr2obsy = PSInfo.cr2obsy;

% 初始化返回结果 
% 初始化返回结果

info = struct('Fcount',uint8(0),'Ncount',uint8(0),'count',uint8(0),'c2dist',single(0),'ymin',single(0));
info.count = uint8(0);
st = true;
ErrCode = uint8(0);
PathPoint  = struct('XPos',single(0),'YPos',single(0),'Headangle',single(0),'Kappa',single(0),'PathLength',single(0),'PathType',Pt_Typ_PathType_en.Initial);
PathPoints = repmat(PathPoint,[1 8]);

% step1 计算大圆圆心1
C1x = vehx + R1*cos(deg2rad(vehth) +single(pi/2));
C1y = vehy + R1*sin(deg2rad(vehth) +single(pi/2));
% step2 圆心1到tpm的距离
a2 =  sin(deg2rad(tpmth));
b2 = -cos(deg2rad(tpmth));
c2 = tpmy*cos(deg2rad(tpmth)) - tpmx*sin(deg2rad(tpmth));
d1 = abs(a2*C1x + b2*C1y + c2)/sqrt(a2^2 + b2^2);
% step3 圆心2沿着tpm方向到圆心1的距离
if (R1-R2)^2-(d1-R2)^2 >= 0
    d2 = sqrt((R1-R2)^2-(d1-R2)^2);
else
    st = false;
    return;
end
% step4 计算切点2的坐标
T2x = C1x + d1*cos(deg2rad(tpmth)-single(pi/2)) + d2*cos(deg2rad(tpmth));
T2y = C1y + d1*sin(deg2rad(tpmth)-single(pi/2)) + d2*sin(deg2rad(tpmth));
% step5 计算圆心2的坐标
C2x = T2x + R2*cos(deg2rad(tpmth)+single(pi/2));
C2y = T2y + R2*sin(deg2rad(tpmth)+single(pi/2));
% step6 计算圆1和圆2的切点
T1cth = atan2(C2y - C1y, C2x-C1x);
T1th  = rad2deg(T1cth + single(pi/2));
T1x = C2x + R2*cos(T1cth);
T1y = C2y + R2*sin(T1cth);
% 判断NP2的右前点或左前点是否小于ymin
NP2LFy = T1y + (VehLength - VehRear)*sin(deg2rad(T1th)) + VehWidth/single(2)*sin(deg2rad(T1th)+single(pi/2));
NP2RFy = T1y + (VehLength - VehRear)*sin(deg2rad(T1th)) + VehWidth/single(2)*sin(deg2rad(T1th)-single(pi/2));
%计算圆心2到左前点和右前点的距离
C2LF_R = sqrt((R2 - VehWidth/single(2))^2 + (VehLength - VehRear)^2);
C2RF_R = sqrt((R2 + VehWidth/single(2))^2 + (VehLength - VehRear)^2);
% NP2的右前点或左前点小于ymin，或左前点和右前点的距离是否大于圆心与ymin之间的距离
if NP2LFy < ymin || NP2RFy < ymin || C2LF_R > C2y - ymin || C2RF_R > C2y - ymin
    st = false;
    ErrCode = uint8(11);
    return;
end

if R2 - sqrt( (C2x -Cr2obsx)^2 + (C2y -Cr2obsy)^2) - VehWidth/single(2) <  Cr2SafeDist
    % 可能与2点碰撞
    st = false;
    ErrCode = uint8(12);
    return;
end
% 判断第一段曲线是否过短
sl = abs(OffCPIP_angdiff(deg2rad(vehth), deg2rad(T1th)))*R1;
if sl < single(0.5)
    st = false;
    ErrCode = uint8(5);
    return;
end
% 返回结果更新
PathPoints(1).XPos = vehx;
PathPoints(1).YPos = vehy;
PathPoints(1).Headangle = vehth;
PathPoints(1).PathType = Pt_Typ_PathType_en.FLCircle;
PathPoints(1).Kappa    = single(1/R1);
PathPoints(1).PathLength = abs(OffCPIP_angdiff(deg2rad(vehth), deg2rad(T1th)))*R1;

PathPoints(2).XPos = T1x;
PathPoints(2).YPos = T1y;
PathPoints(2).Headangle = T1th;
PathPoints(2).PathType = Pt_Typ_PathType_en.BLCircle;
PathPoints(2).Kappa    = single(1/R2);
PathPoints(2).PathLength = abs(OffCPIP_angdiff(deg2rad(T1th), deg2rad(tpmth)))*R2;

PathPoints(3).XPos = T2x;
PathPoints(3).YPos = T2y;
PathPoints(3).Headangle = tpmth;
PathPoints(3).PathType = Pt_Typ_PathType_en.BLine;
PathPoints(3).Kappa    = single(0);
PathPoints(3).PathLength = sqrt((T2x-tpmx)^2+(T2y-tpmy)^2);

PathPoints(4).XPos = tpmx;
PathPoints(4).YPos = tpmy;
PathPoints(4).Headangle = tpmth;
PathPoints(4).PathType = Pt_Typ_PathType_en.Initial;
PathPoints(4).Kappa    = single(0);
PathPoints(4).PathLength = single(0);
info.count = uint8(3);
p1ok = OffCPIP_CheckKeyPathPoint(PathPoints(1),PathPoints(2));
p2ok = OffCPIP_CheckKeyPathPoint(PathPoints(2),PathPoints(3));
p3ok = OffCPIP_CheckKeyPathPoint(PathPoints(3),PathPoints(4));
if ~(p1ok&&p2ok&&p3ok)
    st = false;
end
end
    


